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ABSTRACT 


^  This  report  presents  a  control  scheme  for  accurate  trajectory  following  with  robotic 
manipulators.  The  method  uses  feedforward  control  using  model-based  torques  for  fast  operation 
and  gross  compensation,  and  adaptive  feedback  control  for  correcting  deviations  from  the  desired 
trajectory  under  feedforward  control.  The  adaptive  controller  eliminates  trajectory-following  errors 
in  the  least  squares  sense.  The  control  scheme  takes  into  account  dynamic  nonlinearities  (e.g„ 
coriolis  and  centrifugal  accelerations  and  payload  changes),  geometric  nonlinearities  (e.g.,  nonlinear 
coordinate- transformation  matrices)  and  physical  nonlmearities  (e.g.,  nonlinear  damping)  as  well  as 
dynamic  coupling  in  manipulators.  Computer  simulations  are  presented  to  indicate  the  effectiveness 
and  robustness  of  the  control  scheme.  When  the  desired  trajectory  is  completely  known  before  the 
control  scheme  is  implemented,  then  off-line  computations  can  be  used  to  generate  the  adaptive 
feedback  gains  and  the  computational  efficiency  will  not  be  a  major  limiting  factor  with  this 
control  scheme.  If  real-time  changes  in  the  desired  trajectory  have  to  be  accommodated,  the 
computational  efficiency  has  to  be  improved  using  recursive  relations  to  compute  the  adaptive 
gains.  The  necessary  recursive  relations  are  derived  and  presented  in  this  report. 
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1.  INTRODUCTION 

Many  robot  applications  today  and  in  the  future  will  require  accurate  tracking  of  a  prespecified 
continuous  path.  Common  examples  of  these  tracking  applications  include  seam  tracking,  arc 
welding,  cutting  (laser  and  water  jet),  spray  painting,  contours  inspection,  co-ordinated  parts 
transfer  and  assembly  operations.  These  tracking  paths  are  usually  specified  with  respect  to  the 
end  effector  of  the  robotic  manipulator  and  can  specify  tiajectories  with  respect  to  time  as  well 
as  position.  The  problem  with  achieving  this  objective  of  temporal  path  following  is  that  strong 
nonlinearities  in  the  dynamics  and  geometry,  unknown  parameters,  modeling  errors,  measurement 
errors,  unplanned  changes  in  operating  conditions,  and  other  disturbances  are  present  in  the 
manipulator  and  they  make  accurate  control  of  the  manipulator  very  difficult. 

To  achieve  this  goal  of  accurate  path  following,  a  control  system  is  needed,  which 

1.  accurately  tracks  the  desired  end  effector  trajectory,  often  in  terms  of  time  as 
well  as  position; 

2.  rejects  a  wide  class  of  disturbances,  such  as  parameter  variations  (i.e.,  changing 
payload),  vibrations  and  the  effects  of  static  friction,  and  measurement  errors; 

3.  has  minimal  complexity,  is  computationally  fast,  can  accommodate  a  high 
sampling  rate; 

4.  is  very  reliable,  particularly  in  terms  of  robustness  of  the  control  scheme. 

Many  control  systems,  which  meet  these  requirements  with  different  degrees  of  success,  have  been 
proposed  and  some  have  been  implemented.  The  control  scheme  developed  in  this  report  can 

accurately  follow  a  prespecified  trajectory  while  rejecting  many  classes  of  disturbances  by  using  a 
feedback  control  scheme  that  minimizes  position  and  velocity  deviation  in  the  least  squares  sense 
while  allowing  for  the  changing  of  the  feedback  control  parameters  to  account  for  unknown 
changes  in  payload  or  desired  trajectory.  A  two-link  manipulator  simulation  shows  the 

effectiveness  of  this  control  scheme  for  trajectory  following.  However,  the  computational  effort 
required  with  this  control  scheme  is  high  enough  to  limit  the  maximum  sampling  frequency 

allowed  for  manipulator  control  in  real  time.  Therefore  the  maximum  trajectory-following 

accuracy  that  this  control  scheme  can  achieve  is  also  limited  by  the  computational  effort,  if  the 
desired  trajectory  is  not  known  a  priori,  and  is  changing  in  real  time. 
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1.1  Control  Schemes 

Linear  servo  control  is  the  most  common  type  of  control  in  commercial  use  today  [3].  This 
control  method  involves  having  a  separate  feedback  loop  closed  over  each  manipulator  joint  that 
feedbacks  the  position  (and  sometimes  velocity)  of  that  joint.  This  control  method  has  several 
problems  which  limit  its  commercial  usefulness.  Since  each  control  loop  is  closed  independently 
over  each  manipulator  joint,  it  has  poor  compensation  for  the  dynamic  coupling  (i.e.,  particularly 
coriolis  forces  and  coordinate  coupling)  between  joints  because  the  effect  of  the  motion  of  one 
joint  on  another  is  viewed  as  a  disturbance  which  the  feedback  controller  of  the  second  joint 
must  compensate  for.  At  low  speeds,  these  ‘'disturbance*'  forces  are  small  and  can  be  easily 
compensated  for,  but  at  high  speeds,  these  forces  are  major  components  in  the  dynamics  of  the 
manipulator,  and  the  controller  will  fail  to  totally  reject  these  "disturbances”  and  the  end  effector 
will  no  longer  be  following  the  correct  path  [8].  Another  factor  is  that  the  servo  parameters 
usually  are  tuned  for  one  set  of  operating  conditions  and  can  not  be  changed  to  meet  changing 
conditions  like  payload  variations  during  robot  operation.  Furthermore,  classical  servo  control 
assumes  linear  plants,  which  is  not  close  to  reality  in  the  case  of  robotic  manipulators. 

Other  control  schemes  have  been  proposed  that  eliminate  some  of  these  problems  but  none  have 
been  commercially  implemented.  These  methods  include  Model-Referenced  Adaptive  Control, 
Sliding  Mode  Control  (a  method  of  designing  switching  feedback  regulators  based  on  minimum 
time,  bang-bang  control),  optimal  control,  nonlinear  feedback  control  and  feedforward  control. 
Application  of  these  control  techniques,  particularly  for  real-time  control,  is  hindered  by  the 
complexity  of  the  associated  control  algorithms,  which  increases  the  computation-cycle  lime  and 
decreases  the  control  bandwidth. 

In  model-reference  adaptive  control  [4,  5],  feedback  controller  parameters  are  adaptively 
changed  to  drive  the  manipulator  response  toward  that  of  a  reference  model.  This  reference  model 
need  not  represent  the  actual  manipulator  and  is  chosen  to  suit  the  required  dynamic  behavior.  For 
example,  a  simple  oscillator  (a  linear  second-order  differential  equation)  could  be  used  as  the 
reference  model  for  each  joint  of  the  manipulator. 

Controller  parameters  are  adjusted  according  to  a  differential  law  that  uses  the  error  signal  (the 
difference  between  response  of  the  reference  model  and  the  actual  robot)  as  the  input  There  exist 
several  drawbacks  in  this  scheme,  including  the  following: 

1.  Structure  of  the  feedback  controller  is  not  automatically  generated  by  the  control 
scheme. 

2.  The  adaptive  law  has  to  be  derived  from  scratch  for  the  particular  reference  mode) 
chosen. 

3.  The  control  law  is  completely  independent  of  the  robot  model. 
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4.  The  control  action  has  to  be  generated  faster  than  the  speed  at  which  the  nonlinear 
terms  in  the  robot  change. 

5.  The  adaptive  law  is  derived  on  the  assumption  that  some  of  the  nonlinear  terms  in 
the  robot  model  remain  constant. 

It  is  clear  that  even  though  this  technique  can  produce  satisfactory  results,  particularly  due  to 
the  presence  of  adaptive  feedback  loops,  there  is  no  guarantee  that  the  required  accuracy  is 
obtained  in  a  given  situation  of  trajectory  following. 

A  control  technique  that  strives  to  obtain  linear  behavior  from  a  nonlinear  manipulator  is  known 
as  sliding  mode  control  [9],  In  the  generalized  case  of  this  method  (only  the  two  dimensional 
case  is  presented  by  Klein  and  &  Maney  [9]),  the  state  space  is  partitioned  into  several  regions 
that  are  bounded  by  a  space  trajectory  conformal  to  the  desired  linear  behavior.  The  objective  of 
the  control  would  be  to  drive  the  manipulator  along  the  desired  trajectory.  This  is  accomplished 
by  assigning  a  different  control  law  for  each  region  in  the  partitioned  state  space.  If  the 

manipulator  deviates  from  the  desired  trajectory  and  enters  a  particular  region  of  the  state  space 
the  corresponding  control  law  is  switched  on.  This  will  drive  the  manipulator  back  into  the 

desired  trajectory.  If  it  overshoots,  however,  the  control  law  of  the  new  region  which  the 
manipulator  entered  will  be  automatically  switched  on  to  drive  the  the  manipulator  into  the 

desired  trajectory.  If  the  alternative  control  laws  that  are  assigned  to  the  various  regions  can  be 
switched  on  at  infinite  frequency,  which  is  of  course  not  realistic,  it  is  possible  in  theory,  io 
obtain  ideal  behavior.  In  practice,  however,  the  response  will  chatter  about  the  desired  trajectory. 
The  amplitude  of  chatter  will  depend  on  the  manipulator  dynamics  as  well  as  control  gains  used. 
In  addition  the  switching  frequency  will  depend  on  the  deadband  of  control.  These  shortcomings 
of  sliding  mode  control  can  be  aggravated  by  the  fact  that  the  control  laws  are  selected  in  a 
heuristic  manner,  without  even  employing  a  model  to  represent  the  actual  dynamics  of  the 

manipulator.  At  its  best,  sliding  mode  control  usually  brings  about  tune  delays  (non-synchronous 
response)  in  addition  to  chatter.  This  technique  too,  has  not  been  implemented  in  commercial 
robots. 

In  optimal  control,  the  feedback  control  law  is  designed  by  optimizing  a  suitable  performance 
index  using  a  dynamic  model  for  the  manipulator.  Control  laws  obtained  in  this  manner  can  be 
highly  complex  except  in  a  very  few  special  cases.  A  nonlinear  control  approach  that  has  been 
proposed  for  robotic  manipulator  control  is  aimed  at  obtaining  a  desirable  linear  behavior  from 
the  manipulator  by  employing  a  highly  nonlinear  feedback  law  [6,  1].  Unlike  the  model- 
referenced  adaptive  control  method,  this  control  law  is  derived  from  an  accurate  nonlinear  model 
for  the  robot.  The  main  disadvantage  of  the  method,  as  has  been  warned  by  Asada  &.  Hanafusa, 

[1]  is  the  feedback  law  that  is  so  complex,  it  is  virtually  impossible  to  compute  the  feedback 
parameters  in  real  time  for  practical  robots.  Furthermore,  performance  of  this  nonlinear  control 
system  is  known  to  be  quite  sensitive  to  fidelity  of  the  robot  model  that  is  employed. 
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Z  CONTROL 

This  control  scheme  developed  in  this  report  involves  the  combination  of  feedforward  control 
with  a  least  squares  adaptive  feedback  control  scheme. 

2.1  Feedforward  Control 

This  is  an  open  loop  control  method.  This  method  involves  calculating  the  torques  that  must  be 
applied  at  each  manipulator  joint  so  as  to  have  the  end  effector  follow  the  desired  trajectory. 
These  torques  are  computed  by  from  the  differential  equation  which  models  the  dynamics  of  the 
n-degree  of  freedom  robotic  manipulator.  This  is  known  as  the  inverse-dynamics  problem; 

M(q,W)q  +  f(q.q,W)  «  r(t)  (1) 


where 

W  :  payload 

q  :  vector  of  generalized  joint  positions 

M(q,W)  ;  inertia  matrix  (n  x  n) 

f(q,q,W)  :  vector  representing  centrifugal, 

coriolis,  dissipation  and  gravitational  forces 

r(t)  :  input  torques  or  forces  at  the 
manipulator  joints 

In  practical  manipulators,  input  signals  (e.g.,  field  voltages,  servovalve  commands)  produce 
motor  torques  at  the  joints,  with  some  dynamic  delay.  Motor  torques  are  converted  into  the 
torques  that  are  actually  applied  to  the  links  of  the  manipulator,  with  additional  dynamic  delay. 
Manipulator  displacements  are  a  result  of  these  joint  torques.  It  is  therefore  clear  that,  by  either 
measuring  or  computing  joint  torques  it  is  possible  to  eliminate  part  of  the  delay  in  a 
manipulator  control  system.  Consequently,  feedforward  control  has  the  advantage  of  speeding  up 
the  manipulator  response.  Furthermore,  torque  disturbances  can  be  calculated  or  measured,  they 
can  be  completely  rejected  using  feedforward  control.  A  main  disadvantage  of  feedforward 

control,  in  the  present  context,  is  that  due  to  model  errors  and  unknown  disturbances,  the 

calculated  torque  is  not  the  ideal  torque  and  as  a  result  errors  can  grow  in  an  unstable  manner 

unless  some  form  of  feedback  control  is  used. 

Since  in  inverse  dynamics  a  mathematical  model  of  the  manipulator  is  used  to  calculate  the  joint 
torques  required,  when  these  torques  are  applied  to  the  actual  manipulator  it  might  not  follow  the 
desired  trajectory  accurately.  This  would  be  due  to  the  cumulative  effects  of  modeling 


inaccuracies,  computational  limitations,  and  unaccounted  for  effects  like  vibrations  and  static 
friction.  Therefore,  for  accurate  tracking  using  feedforward  control  a  precise  dynamic  model  has 
to  be  employed  and  the  manipulator  must  be  made  very  rigid  with  strong  structural  links  and 

precision  gear  trains  and  actuators.  Another  problem  with  this  method  is  that  the  computational 

effort  required  to  accurately  compute  the  necessary  torques  in  a  real-time  situation  can  become 
very  significant  if  the  desired  trajectory  is  not  known  a  prion  and  may  not  allow  a  sufficiently 
high  sampling  rate  for  good  control  bandwidth. 

An  adaptive  feedback  is  used  in  the  present  control  method  to  correct  for  these  problems. 

2.2  Background  Theory 

In  most  instances,  feedforward  control  needs  a  feedback  controller  to  correct  for  unaccounted 
disturbances  in  the  system.  Since  linear-servo  control  offers  only  a  limited  ability  to  compensate 
for  nonlinearities,  model  errors,  measurement  errors  and  disturbances  a  more  adaptive  feedback 
controller  was  developed  by  R.P.  Paul  [2].  This  controller  is  based  on  a  nonlinear  coupled 

dynamic  model  of  the  manipulator,  and  therefore  takes  into  account  effects  that  linear  control 
usually  neglects.  It  also  allows  for  updating  the  control  parameters  to  take  care  of  unknown 

external  disturbances  and  payload  variations.  The  basic  block  diagram  for  the  control  system  is 
seen  in  figure  1. 


Figure  1.  Basic  control  diagram  for  the  manipulator 


2.2.1  Linearization 

We  can  linearize  the  nonlinear  set  of  differential  equations  ( 1 )  with  respect  to  small 
perturbations,  3q,  from  the  desired  trajectory,  qj(t),  caused  by  small  torque  disturbances,  3r(t) 


dM  df  df 

M(q.  .W)3q+q. (q.W)3q+ — (q  ,q  ,lV)3q+ — (q  ,q  ,W)3q  *  5r(t) 

d  d  »  d  J  d  ..  o  a 

dq  3q  3q 


where 


r~3M..  ^  ^Mik. 


This  equation  can  be  rearranged  in  vector-matrix  form 

I  0  3q  0  -I 


iq 


0  M  Jq 

d 


-3m  .3f 
q9q  3q 


3f 

3d 


OT 

^rr  3cl 


(2) 


$  r(t)  (3) 


where.  [  ]  ,  denotes  terms  evaluated  in  terms  of  the  desired  trajectory,  q  (t). 

d  d 

This  is,  in  fact,  a  state  space  representation  with  the  state  vector  and  the  input  vector  given  by 
x  *  [  3q,  <5q  ]T  ,  u  -  &r 


thus. 


x  ■  Ax(f)  +  Bu(f) 


(4) 


where,  the  system  matrix 


A(q,  q.q,  W)  *  - 

a 


and  the  input  gain  matrix  is 

0 


B(q  ,  W)  • 

u 

M' 


(5) 


••3M.3t  3» 

M  a  %  F, 


(6) 


Since  what  is  developed  would  be  implemented  as  a  digital  control  scheme,  we  need  the  discrete 
form  of  the  state  space  representation 
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for  —  «  Ax  +  Bud) 

dl 

The  solution  to  this  linear  differential  system  starting  at  can  be  represented  as 

* 

x(t)  *  *<f,fo>x<n  +  J  *(tt0)B(0)u(0)d0  (7) 

I 

o 

which  assuming  time  invariance  in  the  neighborhood  of  the  perturbations,  can  be  expressed  as  the 
set  of  difference  equations 

x(*+l)  *  4*x( Ar)  +  ru(*)  k  *  0,1, 2,3,... 

in  which 

♦  *  eAT  *  state  transition  matrix 

r  *  ^  eA^d/0B  *  input  gain  matrix 
T  *  data  sampling  period 


2.2.2  Minimization 

Since  the  state  vector  x  represents  the  deviation  in  position  and  velocity,  from  the  desired 
trajectory,  then  the  objective  of  the  minimization  is  to  drive  x  to  zero  as  fast  as  possible.  This 
will  be  accomplished  in  the  least  squares  sense  by  using  the  following  objective  index 

Least  Squares  Minimization  Performance  Index  : 

N 

J  ■  2  C*x(Ar)  +  ru<*)JT  Q  [*x<*>  +  ru(*>]  (8) 

k*l 

where  Q  is  a  diagonal  weighting  matrix.  Q  is  used  to  weight  the  relative  importance  of  each 
joint  position  or  velocity.  This  allows  the  motions  of  critical  joints  to  be  more  heavily  weighted 
than  the  motions  of  other  joints. 

This  minimization  is  a  Linear  Quadratic  Regulator  (LQR)  minimization  problem  so  the  optimal 
feedback  gain  should  be  in  some  form  of  the  steady-state  Ricatti  equation. 
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2.2.3  Optimal  Feedback  Gain 

Using  straightforward  calculus  it  can  be  shown  that  the  optimal  control  law  is  given  by 


u<*)  «  -KxU)  (9) 

where  K  *  (  rT  Q  D*1  rT  Q  *  x(rt>  (10) 

It  should  be  noted  that  this  feedback  control  law  is  realizable  if 

rank<  Tt  Q  D  *  n  (11) 

In  particular,  if 

Q  is  positive  definite,  we  must  have 

rank(D  =  n  (12) 

where.  n  -  degrees  of  freedom  of  manipulator 


2.3  Control  Strategy 

The  complete  control  strategy  for  the  manipulator  is  shown  in  figure  2.  First  the  desired  end- 
effector  trajectory  of  the  manipulator  is  jenerated.  Then,  using  some  inverse  kinematics  scheme, 
each  incremental  displacement,  velocity  and  acceleration  of  the  end-effector  is  translated  into  the 
corresponding  motions  of  the  n  joints.  With  the  inverse  dynamics  of  the  manipulator,  the  desired 
gross  torques  for  each  joint  can  be  calculated.  These  torques  are  applied  to  the  actual 
manipulator  in  a  feedforward  manner.  The  actual  joint  positions  and  velocities  are  then  measured 
once  every  period,  T^,  using  resolvers  or  encoders.  The  difference  between  the  actual  and  the 
desired  joint  motions  is  then  multiplied  by  the  optimal  feedback  gain  matrix,  K.  to  produce  the 
vector  of  torque  corrections  that  need  to  be  added  to  the  gross  torque  vector  for  proper  control. 
A  suitable  criterion  is  needed  to  decide  when  to  update  the  feedback  gain  matrix,  K.  In  the 
present  work  the  following  criterion  is  used: 

•  Initially  specify  the  weighting  matrix  Q  and  calculate,  ♦,  and  ,r. 

•  Compute  the  initial  feedback  gain  matrix,  K  using  equation  (10). 

•  Update  the  feedback  gain  matrix,  K,  according  to  the  criterion 

1.  If  i|x||  <  «o  Skip  torque  error  feedback 

2.  If  ||x||  >  *  Update  *,r,Q,  and  K 

3.  If  || x||  >  «3  Excessive  Error,  terminate  operation 

Note  that  t  <  «  <  t  .  The  error  norm  is  defined  as  I  I  x  I  I  *  " ,  «,  I  x  I 

0  12  I  I  I  I  AaMI-l  1  *  l  ' 

•  Update  the  weighting  matrix,  Q,  by  changing  the  diagonal  elements  in  proportion  to 
the  maximum  absolute  value  of  the  state,  ix  ! 

i  max 


Trajectory 

Planning 


Figure  2.  Complete  block  diagram  foT  control  itrategy 
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2.3.1  Stability 

If  the  manipulator  model  is  significantly  different  from  the  actual  robot,  then  the  feedback  law 
could  cause  instability  in  our  control  system.  Stability  is  guaranteed  if  the  closed-loop  state 
transition  matrix,  ♦c,  has  all  its  eigenvalues  inside  the  unit  circle  on  the  Z-plane.  Note  that 

+c «  *  -  r  t<r T  q  r  >*'  r T  q]  ♦ 

0  O  O  0 

where 

♦  ,  r  *  actual  plant  manipulator  matrices 

♦  ,  r  ■  manipulator  model  matrices 


n 


3.  SIMULATION  RESULTS 

The  effectiveness  of  the  control  strategy  presented  in  this  report,  is  examined  using  a  two- 
degree-of-freedom  manipulator.  The  manipulator  equations  are  given  in  Appendix  A.  Two  types  of 
disturbances  were  tested  for  this  control  scheme: 

1.  a  7%  external  disturbance  (figures  3.1  and  5.1),  and 

2.  a  7%  error  in  link  lengths  and  a  9%  error  in  link  inertias  (figure  4.1). 

Typical  results  corresponding  to  these  three  cases  are  presented  in  figures  3,  4,  and  5.  In  all 
three  cases  the  feedforward  control  alone  produces  an  unstable  trajectory  following.  By  adding 
the  adaptive  optimal  feedback  controller  the  actual  trajectory  was  brought  very  close  (8% 
maximum  position  error)  to  the  desired  trajectory. 

It  appears  that  our  control  scheme  satisfies  three  of  the  four  design  goals  for  the  controller: 
accurately  tracks  the  end  effector,  rejects  a  wide  class  of  disturbances,  and  is  very  reliable.  The 
last  goal  is  minimal  complexity,  or  making  the  scheme  computationally  fast  enough  to  allow  an 
adequate  sampling  rate  for  on-line  trajectory  generation  and  control. 

3.1  Two-Link  Manipulator  Results 


End-Effector  Path 


Desired 

Proposed  Feedback 
Feedforward  Only 


Figure  3.1  End-effector  path  with  input  disturbances 


Figure  3.3  X  tad  Y  position  trajectories  of  the  end-effector  with  input  disturbances 


10  20  30  5a  fie  ?0  80  90  ' 100 

Time  (see)  <10*~2> 

Joint  Two  Trajectory 


fcird 

frapesetf  TecAack 
M mwi  lkl]f 


Position  (meters)  C 1 1 > 


X-Component  Trajectory 
of  the  End  Effector 


_ fcsired 

_  Proposed  feedback 

.  feedforvard  Only 


18  28  30  48  38 

Time  (sec) 

68  78  88  98  188 

(10*-2> 

Y-Component  Trajectory 
of  the  End  Effector 

h 

• 

• 

•  • 

• 

e 

••••-"  / 

V 

.  \ 

•  • 

.  ksired 

•  . 

•  •  • 

.  frosted  feedback 
ftedtowd  Ally 

\j  •  • 

y 

8  18  48 

Time  (sec)  (10*-2) 


e  tea 


Figure  4.3  X  and  Y  position  trajectories  of  the  end-effector  with  model  errors 
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Figure  S.l  End-effector  path  with  model  errors 
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4.  COMPUTATIONAL  CONSIDERATIONS 


The  computational  time  that  is  required  to  update  I\  Q  and  K,  will  determine  the  minimum 
error,  <  ,  that  can  be  used  in  the  control  strategy  and  therefore  determine  the  accuracy  of  the 
trajectory  following.  This  update  time  will  therefore  affect  the  maximum  sampling  rate  that  can 
be  used  in  the  feedback  loop  when  on-line  trajectory  generation  is  necessary.  In  many  high 
accuracy  applications,  the  update  time  will  be  the  minimum  sampling  period  allowed,  while  in 
other  less  critical  situations,  the  use  of  the  old  gain  matrix,  K,  during  the  time  needed  to 
calculate  the  new  gain  matrix,  K  will  not  greatly  affect  the  trajectory  error.  It  is  obvious  that 
we  want  to  minimize  the  update  time  so  that  the  maximum  sampling  frequency  is  increased 
enough  to  permit  good  control  bandwidth  for  the  robotic  manipulator. 

The  total  computation  time  can  be  divided  into  three  main  computations: 

•  the  feedforward,  gross  torque  calculation, 

•  the  calculation  of  the  A  and  6  matrices,  and 

•  the  updating  of  ♦,  T,  Q  and  K. 


4.1  Feedback  Controller  Parameter  Calculations 

In  the  two  link  manipulator  simulation ,  Sylvester’s  theorem  [13]  was  used  in  the  calculation  of 
♦.  This  theorem  requires  the  calculation  [11]  of  the  eigenvalues  of  the  system,  and  then  the 
calculation  of  ♦  by  use  of  ♦  *  F^t7  +  F^i7  +  ...  +  F^  nt.  For  complex  eigenvalues,  ♦ 
is  written  as  damped  sine  and  cosine  terms,  and  T  is  calculated  by  a  simple  integration  of  these 
sine  and  cosine  terms.  An  alternate  method  of  4  and  T  calculation  is  the  use  of  the  series 
expansion  method.  Specifically, 


oo 


+  *  T"  Avf/k\ 

k*0 


/  +  at^-aY  + 

2! 


00 


[2  Ak7**'/(A'+1)!]5  -  [r  +  “  +  ~ f  ”•  ]* 


(13) 


(14) 


This  method  is  found  to  be  computationally  faster  because  the  sampling  period,  T,  is 
comparatively  small  so  the  higher  order  terms  are  negligible.  Using  an  mlb  order  expansion  for 
calculating  $  and  F  then  the  number  of  multiplications  for  each  parametric  matrix  is 
^  (2 nY  .  Because  the  computational  expense  is  increasing  exponentially  when  the  number  of 

terms  in  the  expansion  is  increased,  so  a  small  data  sampling  period,  T,  is  very  beneficial 
computationally. 
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The  calculation  of  K 

k  -  (rTor)"  rT  Q  * 

involves  matrix  multiplications,  transposes,  and  the  inversion  of  the  matrix,  (rTQD.  The  inversion 
of  the  matrix  takes  the  longest  to  compute,  and  using  the  Gaussian  elimination  method,  the 
number  of  operations  is  0(n3)  for  an  n  x  n  matrix.  All  these  are  standard  matrix  operations  and 
codes  are  available  to  accomplish  these  operations  in  a  computationally  efficient  manner. 

The  update  calculation  of  Q  is  done  by  changing  the  weights  of  the  diagonal  elements  in 
proportion  to  lx.  ■max,  which  represents  the  maximum  deviation  of  any  joint*s  position  or  velocity 
from  the  desired  motion.  It  is  found  that  in  most  cases,  the  updating  of  Q  does  not  significantly 
affect  the  feedback  gain  matrix,  K,  so  updating  Q  can  be  ignored  if  computational  time  is  very 
critical. 


4.2  Feedforward  Computation 


Many  new  robot  applications  require  on-line  decision  making,  database  access,  and  interaction 
with  other  machines.  Therefore  the  inverse  dynamics  need  to  be  computed  in  real-time  to  obtain 
the  gross  torques  of  the  manipulator  joints,  which  need  to  be  provided  by  the  joint  motors.  The 
standard  method  used  to  derive  the  inverse  dynamics  is  the  standard  Lagrangian  formulation.  Luh, 
Walker  and  Paul  [10]  have  shown  that  this  method  would  require  about  7.9  seconds  on  the  PDP 
11/45  to  calculate  the  gross  torques  for  one  position  of  the  Stanford  Arm  using  an  efficient 
Fortran  program.  This  formulation  requires  a  computational  effort  of  0(n4)  because  the  method  is 
doubly  recursive  with  many  redundant  operations.  The  standard  Lagrangian  method  computes  the 
torques  directly  using 


3WT 


aw.  a2w' 


1  W  V»  at  w ww  .  W  Tv  SJTV 

r.  *  X[  X (tr( — 'J,  — L)>5k  +  XX  {tr{ — Jjj — ~>*kq,>  “  m.9r — 1  v  ] 

j-i  *■  k-i  $<7.  a gk  k*1  »•*  a<7.  ,dq)Bqi  dq.  ^ 


The  computational  time  for  this  is  obviously  too  long,  so  various  methods  of  reducing  the 
number  of  computations  have  been  tried.  Since  most  of  the  computational  effort  is  devoted  to 
calculating  the  triple  sums  involved  in  the  coriolis  and  centrifugal  forces,  many  computation 
schemes  ignore  these  terms.  The  problem  with  this  is  that  at  high  speeds,  the  coriolis  and 
centrifugal  forces  dominate  in  the  manipulator  dynamics  and  therefore  the  burden  of  compensation 
is  increasingly  placed  on  the  feedback  controller.  While  this  method  can  work  at  low  speeds,  at 
high  speeds  this  approximation  could  mean  that  excessive  torques  must  be  applied.  The  controller 
might  not  be  capable  of  doing  this  and  sometimes  burnout  of  equipment  could  result  Alternative 
methods  are  available  using  the  Newton-Euler  [10]  or  Lagrangian  [7]  recursive  relations.  These 
methods  yield  the  same  torques  as  the  standard  Lagrangian  approach,  but  are  computationally 
faster  because  the  standard  Lagrangian  approach  involves  redundant  operations.  These  recursive 
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relations  reduce  the  computational  effort  required  to  0<n).  Lub’s  Newton-Euler  formulation  in 
floating  point  assembly  has  been  shown  to  take  4.S  milliseconds  on  the  PDP  11/43  for  the  torque 
calculation  of  one  position  of  the  Stanford  Arm.  This  will  allow  a  sampling  rate  for  the 
manipulator  of  greater  than  60  Hz  which  insures  good  control  bandwidth  for  the  manipulator.  The 
Lagrangian  recursive  relations  are  presented  here  because  the  computational  formulation  for  the 
feedback  gain  matrix,  K,  is  based  on  this  approach. 

4.2.1  Recursive  Lagrangian  Dynamics 

In  the  following,  the  recursive  Lagrangian  dynamics  procedure  [7]  is  used  to  calculate  the  joint 
torques.  First,  all  the  W  T  terms  are  calculated  using  equations  (17)  and  going  from  i*l  to  i*n. 
Then  the  D.  and  c  terms  are  computed  from  i*n  to  i*l  using  the  forward  recursive  relations 
(16).  Finally,  the  torques  are  computed  using  equation  (13).  This  formulation  has  830n  -  392 
multiplications  and  673n  -  464  additions  which  result  in  4388  multiplications  and  3386  additions 
for  n=6. 

aw.  aw. 

r.  *  ffr( - -  D)  -  gT - '■  c.|  /  *  1 . n  (15) 

'  1  dqt  '  dq{ 

where 


Forward  Recursion 
For  i  *  n,...,l 
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c 
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«  JWT 
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♦  A 
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Backwards  Recursion 
For  i  *  1 . n 


W  «  W  ,  A 

I  I -I  I 

8a  . 

W,  -  W,.,  4,  ♦  W>  —  qt 

dA.  B2A  dA 

W  «  W  .  A  +  2W  , — 1  q  +  W  . - 1  q1  +  W.  , — 1  q 

'  ■"  1  rt8gt  '  ‘  dq. 


(16) 


(17) 


4.3  A  and  B  Matrix  Calculations 

Since  the  A  and  B  matrices  are  based  on  the  linearization  of  the  manipulator  dynamics  about  a 
desired  trajectory,  it  is  suggested  that  an  efficient  formulation  for  their  computations  may  be 
based  on  the  Lagrangian  or  Newton-Euler  recursive  relations  for  the  solution  of  manipulator 
dynamics. 

4.3.1  Derivation 

Looking  at  the  structure  of  the  A  and  B  matrices  it  is  seen  that  three  submatrices  need  to  be 

calculated:  M*1,  — — q  +  — ,  and  —  .  The  Lagrangian  approach  will  be  used  because  the 
3q  dq  dq 

formulation  is  much  clearer  and  the  most  efficient  Lagrangian  relations  are  of  the  same  order  of 
computational  effort  as  the  Newton-Euler  method. 

The  general  Lagrangian  formulation  for  the  generalized  forces,  r.,  for  and  n-link  manipulator  is 

"  ‘  BW  BwJ  Bw  B2WT  Bw 


which  also  can  be  written  in  the  form  [12] 
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j-l  k-l 
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andwhere 


cor  i  ol  is  and  centripetal  forces 
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4.3.2  Linearized  Matrices 


The  three  matrices,  M*1,  —  q  +  — .  and  — ,  are  necessary  to  compute  results  from  the 

3q  3q  3q 

linearization  of  the  inverse  dynamics  with  respect  to  small  perturbations,  5 q. 


i  \  r3M  "  1 

(a)  I —  q+ — 
oq  3q  J 


term: 


The  first  matrix  computation  formulation  is  f  — —  q+— -  1.  This  matrix  is  derived  by  taking  the 

*•3 q  Oq  ^ 

partial  derivative  of  the  generalized  forces  with  respect  to  the  joints’  position  vector.  So 
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But  Waters  [14]  proved  that  instead  of  the  standard  Lagrangian,  the  generalized  forces  can  be 
expressed  in  a  form  that  will  permit  several  backward  recursive  relations  to  be  derived  that  will 
reduce  the  computational  effort  to  0(n2). 
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where  the  backward  recursive  relations  for  velocities  W  and  accelerations  W  are 
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Using  the  same  formulation  for  the  generalized  forces,  the  derivative  of  the  generalized  forces 
can  be  expressed  as 
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Now 
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Consequently,  the  matrix  formulation  can  be  written  as 
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for  i  =  1 . n  and  j  *  1 . n 

Now  a  forward  recursive  relation  can  be  developed  by  noting  that 
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Therefore  for  the  two  cases  of  the  double  derivative  we  obtain 
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Similarly  for  j  >  i 
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Because  of  the  symmetry  of  the  equations  of  the  double  derivative,  only  the  case  i 
considered  in  what  follows. 
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j  will  be 


Rewriting  the  matrix  formulation  as 
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j 

3wt 
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'  '  dqj  *' 

Now  for  i2j  the  matrix  is  simply  written  as 

AM  Af  3JW  3W. 

r §£  q+—  1  -  \tr( - -  D .)*  N.)  - 

lBq  3<7J,j  L  3(7,3^  3<7, 

By  a  similar  procedure  we  get 


for  j  *  i 


.aw  1  .  r„(^_L  0)*  '  «,  -  , 

L<n_  i-J'J  L  1  J 


28 


where 


JWT  *  A  D 

(34) 

j  j  ri  i*t 

m  V  ♦  A  c 

(35) 

J  j  J*1 

3wT 

1— ±-  +  A  N 

(36) 

J  3q  j*1 

j 

(b) 


term: 


Using  a  procedure  similar  to  what  is  given  in  the  previous  section,  the  [fO  tom  “ 
simply  formulated  as  a  set  of  linear  recursive  backward  and  forward  relations.  This  matrix  term 
is  derived  by  taking  the  partial  derivative  of  the  generalized  forces  with  respect  to  the  joints’ 
velocity  vector.  So 


r-1  -  —  r 

L3iJ"  a<7, 


1,...,^,  y*l,.../r 


(37) 


Now  using  Waters  generalized  forces  formulation,  the  matrix  becomes 


r3f. 


L 


d(f 


l  -  z 

I  P-1 


—  dwB  awDT 
^  — ) 

p..  dq.  dq 


(38) 


3Wp 

If  j  >  p  then  -  *  0 

Consequently  the  matrix  equations  are  written  as 


r-1-  t  w— 1  j„  — ^ 

dq  J  p-ma*  i.j  dq,  dqj 


p -max  i.j 

Consider  first  the  case  of 
If  i  >  j 


3W_T 
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(39) 


1  -  >  tr( -  W  J  - -) 

La<r|,J  dg  p  p  dq 
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rdf  i 

r—T.  *  tr( 

La^J,J  dq{  - 


awT-L,  awDT 

-z *P  ■  p  ■ 


(41) 
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Now,  it  can  be  shown  that 


9w  T 

9wT 

_ _ p_ 

* 

p 

a<7, 

;  to  the  reformulation 

r-T  - 

L3iJ" 

BW ,TJL 
■  fr(  2- 

a<7j 

BW  T 

iw  j  — > 

p  p 


a?, 

that  produces  the  forward  recursive  relation  by  letting 

"  BW1 

0,  -  Z  wp  JP  — 

p-.  Bq. 


°i  9  Ai*X  +  J 


BW. 

‘ 


So  the  matrix  compuation  is  simply  formulated  as 

rBf1  avv,  „ 

La*Jj  a?,  ' 

Considering  the  other  case  and  by  applying  similar  arguments  we  get 
for  j  2:  / 

,3/,  Swr  3wT 

^3<?  p-j  3  qt  dq. 


r3 /-  aw/  ’  3wT 

M,  *  w,  Z  w,  j,  — > 

Bq  Bqt  p*j  9^ 

Then  the  matrix  is  formulated  as 


tr(- 


BW 

I 


'W  Q) 

i  ) 


BwT 

A  Q  +  J  - L 

j*i  j»i  j 

a<7. 


(42) 


(43) 


(44) 


(45) 


(46) 


(47) 


(48) 


(c)  M. .  t«rm: 
•j 


The  next  matrix  to  be  calculated  is  the  inertia  matrix,  M.  The  recursive  relations  are  derived  in 
the  same  manner  as  the  other  matices.  Specifically, 


[f] 


V 

M  •  D  •  V  tr( — E./ - 

>J  u  ^  i  p 

p'OMXI.j  ^<7  Cty 


aw  awT 


For  i  £  j 


aw  awT 


Af 
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dq 


'W  J  - 

p  p 


.  a<7i 


aw 


w  iTT 

M  «  fr< - >  'W  J - ) 

,J  a?h  pp  a<7 

1  j 

the  forward  recursive  relation  is 

"  awT 


awT 


*,-1 


■w  j  — 

P  p  -s 

p-i  dq. 


) 


aw 


/»  *  A  ,P.  ,  +  J- 
1  i*i  1*1  1 


d<7. 


and  the  matrix  is  computed  simply  by 

aw 


Af .  »  tr(—  A) 

a*. 


for  j  £  i 


”  aw 

M  =  y  tr( - 1  ‘W  ‘ 

ij  .  j 


awT 


p-j  a<7 

trix  I 
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W  J  - 

p  p 


d<7. 


-) 


■  1  j 

In  this  case  the  matrix  formulation  and  forward  recursive  relations  are 


M  .  •  tr( -  'W  P) 

>J  -V  J  J 

a<7, 

awT 


p  -  4  ,/». ,  +  j- 

j  j*i  r>  j 


d<7( 
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(50) 

(51) 


(52) 

(53) 

(54) 

(55) 

(56) 


dw  3w 

The  last  terms  that  need  to  be  calculated  are  the  -r-?  and  -r— 

dq.  dq^ 

recursive  relations  needed  to  calculate  these  terms  are  now  presented 


terms.  The  backward  linear 


aw 
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j  *  1 . p~  1 


a:w  34  Ba 

- —  =  w  . — J  w  — - 

a<7 pBq.  r  3(7,  ”■  3(7p 


for  j  - 


4.4  The  Summary  of  Recursive  Relations 

Bm  f  Bf 

Now,  to  summarize  the  procedure  for  computing  the  M  ,  — — q  +  — ,  and  — ,  matrices.  First, 

Bq  B  q  3q 

the  backward  recursive  relations  (64)  are  used  to  compute  all  the  W  T  terms  from  i  z  1  to  i=n. 

3w,T  3w,T  3w,T 

Then  all  the  — - — ,  — — .  — —  terms  are  computed  by  the  recursive  relations  (65),  (66)  and  (67) 

3oj  do,  3q. 

for  i*l  to  i*n  and  j-l  to  j*n.  but  only  for  the  cases  of  i  t  j.  Next  the  forward  recursive 
relations  (68)  and  (69)  are  used  to  calculate  D,  and  c  for  i*n  to  i«l,  and  relations  (70).  (71) 

i  i 

and  (72)  are  used  to  calculate  P  ,  Q..,  N..  for  j*I  to  j*i.  Finally,  the  necessary  control 

matrices,  M"\  and  ^  are  computed  by  (73),  (74),  (75),  (76),  (77)  and  (78)  for 

3  dq  Bq 

q 

i*  1  to  i*n  and  j*l  to  j*n.  Noting  that  many  of  the  terms  are  the  same  as  those  calculated  for 
the  feedforward  computations  if  the  feedforward  calculation  is  incorporated  in  the  control  loop, 
then  many  of  these  computations  need  not  be  repeated. 


4.4.1  Backwards  Recursion 

For  i  *  1 . n 


W .  -  W  ,  A 

■  i-i  i 


W.  *  W  .  A  +  W  , -  <7 

I  1-1  I  I-I*  M 

37, 

.34,  3Z4  34, 
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37,  37,*  3(7, 


(64) 


4.4.2  Forward  Recursion 


For  i  *  n.....l 
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I'M  1*1 


For  j  »  1 
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/>  ..  +  J 

i+lj 
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N  -  A  N  + 

/Vij  i+l  i+lj 


For  .  i=1,...n 

(a)  M.j  term: 


For  i  £  j 
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For  j  St  i 
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aw 

tr( - 1  P  ) 

a<7i 


aw 

tr{ - 1  jW  P 


p3m  ..  3f  -j 
(b)  I —  q+—  j 

L3q  3q  J 


term. 
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If  j  *  i 


3 M  ..  3/  n 
-  q+—  I 

X 

32w.  3w.  3  2w. 

ftr( - L  £>  )+  tr( - 1  N ..)  -  $T - L 

3<7 

3(7  J 

1 

3<73<7j  '  3<7j  dqpQj 

3 M  ..  3/  1 

-  q+—  1 

3  2w  dw  3  '-w. 

r tr( - L  D{)+  tr( - 1  N..)  -  gT - - 

3<7 

3  q  J 

»j 

dqdqi  dq.t  dqfy 

(75) 


(76) 


( c ) 
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for  i  2  j 


a?JiJ 
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tr(- 


3tV. 

3^ 


Q) 

ij 


(77) 


for  j  >  i 


■3/ 

*a<7 


J. 


tr(~ 


3  wi 


W.Q..) 

j  '< 


(78) 


The  number  of  multiplications  involved  with  the  matrix  calculations  is  1062n  -  1021n  -  128 
and  the  number  of  additions  is  1037n2  -  62 In  -96.  This  means  that  for  n*6,  the  number  of 
multiplications  is  40,594  and  the  number  of  additions  is  37,926  for  each  update  of  the  A  and  B 
matrices.  Therefore,  the  number  of  multiplications  and  additions  is  of  n2  dependence  and  for  n*6 
the  number  of  operations  is  10  times  the  number  of  operations  involved  in  the  recursive 
Lagrangian  dynamics  relations. 


4.5  Recursive  Parametric  Matrices  Using  3x3  Matrices 

The  previous  formulation  reduces  the  computational  effort  to  0(n2)  for  each  matrix,  which  is 
the  lowest  order  that  can  be  achieved.  The  only  way  to  further  reduce  the  computational  cost  is 
to  use  3x3  rotation  matrices  instead  of  4  x  4  rotation-translation  matrices.  The  4x4  matrices 
are  inefficient  because  of  some  sparseness  and  because  of  the  combination  of  translation  with 
rotation  [7],  The  4x4  matrices  require  64  multiplications  for  each  matrix  multiplication,  while 
3x3  matrices  only  require  27  multiplications,  so  a  58%  reduction  in  coefficient  multiplications 
is  effected. 

The  3x3  rotation  matrix  A.  relates  the  orientations  of  coordinate  systems  j-1  and  j,  and  W. 


36 


and  'W  are  defined  as  before  except  the  A  matrices  are  3  x  3. 


Using  these  relations,  the 


derivation  of  the  formulations  for  computing 


♦ 


8f  8f 
Fq  aDd  Fq 


using  3x3  matrices  is 


presented  in  Appendix  B.  The  procedure  for  calculating 
using  3x3  rotation  matrices  is  now  summarized. 


8M„  df  8f 

the  M  ,  — — q  ♦  r-,  and  — ,  matrices 

oq  oq  oq 

First,  the  backward  relations  (64),  (65), 


8w.T  8wt  8wt 


(66),  (67)  and  (79)  are  used  to  compute  all  the  -z  — ,  -r-1-, 


8p(T  8p.T  8p(T 

dq,  ’  Sq/  3qj  ’  *nd  8q,’  Sq^’  8^’ 


terms  for  i*l  to  i*n  and  j=l  to  j*l.  Next,  the  forward  recursive  relations  (80),  (81)  and  (82) 
are  used  to  calculate  D,  e  and  c  for  i«n  to  i*l,  and  relations  (83).  (84),  (85),  (86),  (87)  and 

i  i  i 

(88)  are  used  to  calculate  P  ,  k  ,  Q  ,  b  ,  N  I  ,  for  j*l  to  j=i.  Finally,  the  necessary  control 

ij  ij  ij  ij  IJ  ij 


matrices,  M’ 


8M 

’  aT 


8f  8f 

— ,  and  r-r-are  computed  by  (89),  (90),  (91),  (92),  (93)  and  (94)  for 

8q  8q 


i=l  to  i*n  and  j»l  to  jan. 


4.5.1  Backwards  Recursion 


8wT  8wT  8wt 

The  .  .  -z-5 — terms  are  calculated  with  the  same  recurrence  relations  (64),  (65),  (66) 

oq  oq.  oq 

j  J  j 

and  (67)  as  before  except  the  matrices  are  now  3x3. 
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tr( - 1  'W  P  ) 

■j 

3<7j  J  " 
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L3q  3q 
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If  j  *  i 
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3q 


for  i  2:  j 

r3fn  9Wt 

|  — |  »  frt - Q)  (93) 

L3v  J  3q,  ,J 

for  j  >  i 

rdf-i  *Wi 

I  «  tr( -  W.O.)  (94) 

L3qJ,J  dg.  J  " 

The  Dumber  of  multiplications  involved  with  the  recursive  3x3  relations  is  739nz  ♦  62n  -54 
and  the  number  of  additions  is  (1161/2)n2  -  ( 1 9/ 2 )n  -  36.  For  n*6  the  number  of 

multiplications  for  each  update  of  A  and  B  is  26922  and  the  number  of  additions  is  20805.  This 

is  a  greater  than  40%  reduction  in  the  number  of  operations  over  using  4x4  rotation-translation 
matrices. 
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5.  CONCLUSION 

This  report  has  presented  a  control  scheme  for  accurate  trajectory  following  with  robotic 
manipulators.  The  technique  has  been  based  on  the  use  of  measured  joint  displacements  and 
velocities  to  generate  corrective  torques  through  an  adaptive  controller  that  eliminates  deviations 
of  the  manipulator  from  the  desired  trajectory  under  feedforward  control,  in  the  least  squares 
sense.  The  controller  has  taken  into  account  dynamic  nonlinearities  (coriolis  and  centrifugal 
accelerations,  pay-load  change,  etc.),  geometric  nonlinearities  (nonlinear  transformation  matrices), 
physical  nonlinearities  <e.g.,  coulomb  damping),  dynamic  coupling  between  joints,  and  real-time 
changes  in  the  desired  trajectory.  Simulation  results  have  been  presented  for  a  two-degree-of- 
freedom  manipulator.  These  results  have  indicated  the  effectiveness  and  robustness  of  the 
controller.  The  stability  issue  has  been  addressed.  Recursive  relations  have  been  developed  to 
compute  the  adaptive  feedback  gains,  thereby  improving  the  computational  efficiency  of  the  scheme 
that  makes  the  controller  feasible  under  real-time  changes  in  the  desired  trajectory.  Two  methods 
of  deriving  the  recursive  relations  based  on  Lagrangian  dynamics  have  been  presented:  (i)  using 
4x4  rotation-translation  matrices,  and  (ii)  using  3x3  rotation  matrices.  For  a  six  degree-of- 
freedom  manipulator,  the  3x3  Lagrangian  recursive  relations  involve  47,727  operations,  which  is 
41%  more  efficient  than  the  alternative  method  of  using  4x4  rotation-translation  matrices.  The 
number  of  operations  involved  in  updating  the  feedback  gain  matrix  would  limit  the  maximum 
update  frequency  to  about  3  Hz  when  used  with  computers  like  the  PDP  11  for  six  degree-of- 
freedom  manipulators. 
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APPENDIX  A.  TWO-LINK  MANIPULATOR 

In  this  appendix  we  formulated  a  dynamic  model  for  a  two-link  manipulator. 


Figure  A.1  Nomenclature  for  the  two-link  manipulator 
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A.1  Kinematics 
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Joint  Accelerations 
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A.  2  Dynamics 
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AM,(  «  \*smdVi*iM9+dJ 

AM2J  ■  -I2*sin0}0 ,*I2*cos0,02l 

Af„  ’  ~2l'%ineu> 

Afj2  «  -21  •»ta0  <0  ♦*  > 

A,„  ■  2V9, 


45 


APPENDIX  B.  RECURSIVE  CONTROL  PARAMETERS  WITH  3  X 
3  MATRICES 


Id  this  appendix  the  formulation  for  the  three  matrices, 
using  3x3  rotation  matrices. 


M 


-i 


5m  ••  df 


is  developed 


Figure  B.l  3x3  Vector  definitions 

p.:  vector  from  hase  coordinate  origin  to  the  joint  i  coordinate  origin 

p  *:  vector  from  the  origin  i-1  to  coordinate  origin  i. 

r;  vector  from  the  base  coordinate  origin  to  the  link  i  center  of  mass 

r  *:  vector  from  coordinate  origin  i  to  the  link  i  center  of  mass 

n:  r*  /  m 

I  I 

*Wk:  defined  as  before  except  it  is  composed  of  3  x  3  rotation  matrices. 


(B.l) 


9q  8q 


[f  I  -  I  [ 


'  dq  dq 


d2po 

p  ••  x 

tr(  m - p‘  +  m 

Pdqdq} 


dqi  dqi 


B2P  dp  dWj  d2Wa 

_JjL  Pn%T  +  _Jpfl  t_L  +  - —  p„  Tp  t  + 

dqdq  P  P  3^  P  dq^  dqdq. 


dqdq j  3^  3^  3^3?. 

aw  J  a£T  a2w  ..  aw  awB  a2w 
- E  p„  T_£_+ - -j  w  + - Ej - )-m  oT -  tv  l 

a<7.  P  3?J  dqdq}  P  P  3?,  rdqi  ^  Pj 

Now  for  the  case  where  i  ^  j 
Pp  =  p.  +  W>. 

3PP  3*. 

— "  T"  A 

3^  37j 

aw  aw 

— - - -  'W 

dqt  dq.  P 


For  i  2:  j 

?P,  3!w, 

1  S  - 

dqdqj  dqdq. 

a2w  82W 

_ P_  _ 

dqdq.  dqdq^ 


^  a  a2w  n 

I  ■  "E TT  ^  (”>AI  ♦  W*p + ' Wp7  + '"W 

■"  a<7  dqJ  dqdqi  n 

aw  "  dpj  aw  apT  aw 

a,,tr  ’  ’  a?J  ?  '  a*,  r  a? 

a2w  " 

-oT - >  m'W  vr 


•  Kilt. 


,•19  ( 


for  i  £  j 


a,  8W 

I"— 1  «  tr( - 1  Q  ) 

a9j  ' 

Similarly  for  j  >  i 

a,  BW 

f— T  »  tr( - 1  'W.Q) 

L-n-J'j  a  j  j 

a<7  3<7( 

8p.T  awT 

Q  *  A  Q  +  *p  b  +  */7T -  +  J  .' — ~ 

i  i*i  i* •  *>•  i»«  j  J  ^ 

By  a  similar  procedure  we  obtain 


(c)  M..  term: 


For  i  2:  j 


M  -  tr( -  P) 

a<7, 


3p,T  3vv.T 

+  +  ,/7iT—  +  JiT~ 

3^  3  9i 


for  j  St  i 


M  -  tr( -  'W  P) 

"  3<7j  j  j 


3p.T  aw1 

P  m  A  P  +  ifl  t  +  i/»  -  +  J  "  — 

j  j*.  j*i  ^VI  i*«  j  dq^  i  3^ 


where 


apsT  awj 

k.  «  Ar  +  m. — -  +  ‘/J.1 - 

i  i*l  ■  i 

3pj  dqt 

The  last  new  terms  that  need  to  be  calculated  are  the  p  terms. 


